#include "hls_poscmd_server/hls_poscmd_server.h"

hls_poscmd_server::hls_poscmd_server(ros::NodeHandle &nh)
{
    nh_ = nh;

    recieved_takeoffcmd_flag_ = false;
    recieved_landingcmd_flag_ = false;
    state_ = INIT;
    pos_recieved_.setZero();

    nh_.getParam("free_yaw", free_yaw_);
    nh_.getParam("init_x", init_pos_(0));
    nh_.getParam("init_y", init_pos_(1));
    nh_.getParam("init_z", init_pos_(2));
    setSetpointPosition(init_pos_(0), init_pos_(1), init_pos_(2));
    setSetpointRaw(init_pos_(0), init_pos_(1), init_pos_(2),
                    0, 0, 0,
                    0, 0, 0,
                    0, 0);
    
    std::cout << "[hls_poscmd_server]free_yaw: " << free_yaw_ << " init pos = " << init_pos_.transpose() << std::endl;

    offboard_pub_ = nh_.advertise<std_msgs::Bool>("shieh_off_board", 10);
    arming_pub_ = nh_.advertise<std_msgs::Bool>("shieh_arming", 10);
    landing_pub_ = nh_.advertise<std_msgs::Bool>("shieh_land", 10);
    setpoint_position_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("setpoint_position_topic", 10);
    setpoint_raw_pub_ = nh_.advertise<mavros_msgs::PositionTarget>("setpoint_raw_topic", 10);
    takeoff_finish_pub_ = nh_.advertise<std_msgs::Bool>("takeoff_finish", 10);

    pos_sub_ = nh_.subscribe("pos_topic", 20, &hls_poscmd_server::RcvPosCallback, this);
    takeoffcmd_sub_ = nh_.subscribe("takeoffcmd_topic", 10, &hls_poscmd_server::RcvTakeoffCmdCallback, this);
    landingcmd_sub_ = nh_.subscribe("landingcmd_topic", 10, &hls_poscmd_server::RcvLandingCmdCallback, this);
    /**Poscmd Topic Subscriber Here**********************************************************************************/
    poscmd_sub_ = nh_.subscribe("poscmd_topic", 20, &hls_poscmd_server::RcvPoscmdCallback, this);
    /****************************************************************************************************************/

    hls_poscmd_server_rosTimer_ = nh_.createTimer(ros::Duration(0.05), &hls_poscmd_server::PoscmdServerTimerCallback, this);

}

void hls_poscmd_server::setSetpointPosition(double x, double y, double z)
{
    setpoint_position_(0) = x;
    setpoint_position_(1) = y;
    setpoint_position_(2) = z;   
}

void hls_poscmd_server::setSetpointRaw(double pos_x, double pos_y, double pos_z,
                        double vel_x, double vel_y, double vel_z,
                        double acc_x, double acc_y, double acc_z,
                        float yaw, float yaw_rate)
{
    setpoint_raw_.position_(0) = pos_x;
    setpoint_raw_.position_(1) = pos_y;
    setpoint_raw_.position_(2) = pos_z;

    setpoint_raw_.velocity_(0) = vel_x;
    setpoint_raw_.velocity_(1) = vel_y;
    setpoint_raw_.velocity_(2) = vel_z;

    setpoint_raw_.acceleration_(0) = acc_x;
    setpoint_raw_.acceleration_(1) = acc_y;
    setpoint_raw_.acceleration_(2) = acc_z;

    if (free_yaw_)
    {
        setpoint_raw_.yaw_ = yaw;
        setpoint_raw_.yaw_rate_ = yaw_rate;
    }
    else
    {
        setpoint_raw_.yaw_ = 0;
        setpoint_raw_.yaw_rate_ = 0;
    }
}

void hls_poscmd_server::pubSetpointPosition()
{
    geometry_msgs::PoseStamped msg;
    msg.pose.position.x = setpoint_position_(0);
    msg.pose.position.y = setpoint_position_(1);
    msg.pose.position.z = setpoint_position_(2);
    // std::cout << msg.pose.position.x << " " << msg.pose.position.y << " " << msg.pose.position.z << std::endl;
    setpoint_position_pub_.publish(msg);
}

void hls_poscmd_server::pubSetpointRaw()
{
    mavros_msgs::PositionTarget msg;

    msg.position.x = setpoint_raw_.position_(0);
    msg.position.y = setpoint_raw_.position_(1);
    msg.position.z = setpoint_raw_.position_(2);

    msg.velocity.x = setpoint_raw_.velocity_(0);
    msg.velocity.y = setpoint_raw_.velocity_(1);
    msg.velocity.z = setpoint_raw_.velocity_(2);

    msg.acceleration_or_force.x = setpoint_raw_.acceleration_(0);
    msg.acceleration_or_force.y = setpoint_raw_.acceleration_(1);
    msg.acceleration_or_force.z = setpoint_raw_.acceleration_(2);

    msg.yaw = setpoint_raw_.yaw_;
    msg.yaw_rate = setpoint_raw_.yaw_rate_;

    // std::cout << setpoint_raw_.position_.transpose() << "\n" 
    //     << setpoint_raw_.velocity_.transpose() << "\n" 
    //     << setpoint_raw_.acceleration_.transpose() << "\n" << std::endl;

    setpoint_raw_pub_.publish(msg);
}

void hls_poscmd_server::RcvPosCallback(const geometry_msgs::PoseStamped &msg)
{
    pos_recieved_(0) = msg.pose.position.x;
    pos_recieved_(1) = msg.pose.position.y;
    pos_recieved_(2) = msg.pose.position.z;
}

void hls_poscmd_server::RcvTakeoffCmdCallback(const std_msgs::BoolConstPtr &msg)
{
    if (msg->data && !recieved_takeoffcmd_flag_)
    {
        recieved_takeoffcmd_flag_ = true;
        recieved_landingcmd_flag_ = false;
        ROS_WARN("[hls_poscmd_server]Recieved takeoff cmd!");
    }
}

void hls_poscmd_server::RcvLandingCmdCallback(const std_msgs::BoolConstPtr &msg)
{
    if (msg->data && !recieved_landingcmd_flag_)
    {
        recieved_landingcmd_flag_ = true;
        recieved_takeoffcmd_flag_ = false;
        ROS_WARN("[hls_poscmd_server]Recieved landing cmd!");
    }
}

/**Poscmd Topic Callback Here**********************************************************************************/
void hls_poscmd_server::RcvPoscmdCallback(const quadrotor_msgs::PositionCommandConstPtr &msg)
{
    if (state_ == READY)
    {
        setSetpointRaw( msg->position.x, msg->position.y, msg->position.z,
                        msg->velocity.x, msg->velocity.y, msg->velocity.z,
                        msg->acceleration.x, msg->acceleration.y, msg->acceleration.z,
                        msg->yaw, msg->yaw_dot);
    }
}
/****************************************************************************************************************/

void hls_poscmd_server::PoscmdServerTimerCallback(const ros::TimerEvent &)
{
    std_msgs::Bool drone2mavros_cmd;
    std_msgs::Bool takeoff_finish_msg;
    Eigen::Vector3d dist;
    
    if (!recieved_takeoffcmd_flag_)     // Not allowed to take off yet.
    {
        return;
    }
    else                                // Allowed to take off.
    {
        switch (state_)
        {
        case INIT:
            pubSetpointPosition();      // /mavros/setpoint_position/local must be published before offboard.
            drone2mavros_cmd.data = true;
            offboard_pub_.publish(drone2mavros_cmd);
            arming_pub_.publish(drone2mavros_cmd);

            state_ = CLIMB;
            ROS_INFO("[hls_poscmd_server]Set offboard and arming, CLIMB.");
            break;
        case CLIMB:
            pubSetpointPosition();

            dist = pos_recieved_ - init_pos_;
            if (dist.norm() < TAKEOFF_SUCCESS_THRE)
            {
                state_ = READY;
                takeoff_finish_msg.data = true;
                takeoff_finish_pub_.publish(takeoff_finish_msg);
                ROS_INFO("[hls_poscmd_server]Takeoff successfully, READY.");
            }
            break;
        case READY:
            pubSetpointRaw();

            if (recieved_landingcmd_flag_)
            {
                state_ = INIT;
                ROS_WARN("[hls_poscmd_server]Landing request, INIT, takeoff_flag is set to %d.", recieved_takeoffcmd_flag_);
            }
            break;
        default:
            ROS_ERROR("[hls_poscmd_server]State Error!");
            break;
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "hls_poscmd_server_node");
    ros::NodeHandle nh("~");

    hls_poscmd_server flight_test(nh);

    ros::spin();
    return 0;
}